#pragma once
#ifndef FRONTEND_H
#define FRONTEND_H

#include <opencv2/features2d.hpp>
#include"config.h"
#include"frame.h"
#include"camera.h"
#include"algorithm.h"
#include"map.h"
#include"show_pub.h"
#include"g2o_types.h"

namespace test_visual_odom{

enum class  FrontendStatus { INITING, TRACKING_GOOD, TRACKING_BAD, LOST };

class Frontend{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    typedef shared_ptr<Frontend> Ptr;
    Frontend();
    Frontend(Config::Ptr p_config, Camera::Ptr p_camera_left, Camera::Ptr p_camera_right);

    bool addFrame(Frame::Ptr frame);

    void setShowPub(ShowPub::Ptr p_show_pub){
        p_show_pub_ = p_show_pub;
    };

private:
    bool stereoInit();
    int detectFeatures();
    int findFeaturesInRight();
    bool buildInitMap();
    bool leftImgTrack();
    int trackLastFrame(); //使用光流
    int estimateCurrentPose();
    bool featurePointsFitForKeyframe();
    int triangulateNewPoints();

    FrontendStatus status_ = FrontendStatus::INITING;

    Frame::Ptr p_current_frame_ = nullptr;
    Frame::Ptr p_last_frame_ = nullptr;
    Camera::Ptr p_camera_left_ = nullptr;   //左目参数以及转换
    Camera::Ptr p_camera_right_ = nullptr;  

    Map::Ptr map_ = nullptr;

    int num_features_ = 200;
    int num_features_init_ = 100;
    int num_features_tracking_ = 50;
    int num_features_tracking_bad_ = 20;
    int num_features_needed_for_keyframe_ = 80;
    cv::Ptr<cv::GFTTDetector> gftt_;  

    ShowPub::Ptr p_show_pub_;

    //track
    SE3 relative_motion_;  // 当前帧与上一帧的相对运动，用于估计当前帧pose初值
    int tracking_inliers_ = 0;
};

}

#endif
